import cv2
import numpy as np
# 相机内参矩阵
K = np.array([[465.89830246, 0.0, 321.75423926],
              [0.0, 465.70203766, 234.38544949],
              [0.0, 0.0, 1.0]])
# 畸变系数
dist_coeffs = np.array([-1.56124824e-03, 3.56434644e-01, -2.32354193e-04, -1.35109750e-03, -7.74506460e-01])

# 输入像素坐标原点
u0_image = 320 # 用实际的图像坐标替换
v0_image = 240  # 用实际的图像坐标替换
#输入想要标记的像素坐标
u_image = 340 # 用实际的图像坐标替换
v_image = 340  # 用实际的图像坐标替换
#像素实际物理尺寸
dx = 0.014
dy = 0.014


# 将像素坐标转换为校正后的像素坐标
undistorted_coords = cv2.undistortPoints(np.array([[u_image, u_image]], dtype=np.float32), K, dist_coeffs)

# 输出图像坐标
print("像素坐标 (u, v)pixel:", u_image, v_image)
x_image = (u_image - u0_image) * dx 
y_image = (v_image - v0_image) * dy 
print("图像坐标 (x, y)mm:", x_image, y_image)

# 使用相机内参的逆来将校正后的像素坐标转换为相机坐标
camera_coords = np.dot(np.linalg.inv(K), np.array([undistorted_coords[0][0][0], undistorted_coords[0][0][1], 0.0]))
# 输出相机坐标
x_camera = camera_coords[0]
y_camera = camera_coords[1]
z_camera = camera_coords[2]

print("相机坐标 (x, y, z)mm:", x_camera, y_camera, z_camera)
